﻿/// 使用Eigen封装的数学模型
///
/// ME_NO_VTK 不需要VTK的时候定义该变量
/// ME_NO_CV 不需要CV的时候定义该变量
/// 
#ifndef MEEIGENMATH_H
#define MEEIGENMATH_H

#include "matrixd.h"

#include <Eigen/Dense>
#include <opencv2/core.hpp>
#ifndef ME_NO_VTK
#include <vtkMatrix4x4.h>
#include <vtkSmartPointer.h>
#endif

namespace {
/// ostream Eigen::Vector3d
static inline
std::ostream& operator << (std::ostream& out, const std::vector<Eigen::Vector3d>& vec)
{
    out << "[";
    unsigned int i;
    for (i =0; i < vec.size()-1; i++) {
        out << vec[i].transpose() << "; ";
    }
    out << vec[i] << "]";
    return out;
}

}
class MeEigenMath
{
public:
    /// 均值
    static Eigen::Vector3d mean(std::vector<Eigen::Vector3d> &pts);

    static double distance(const Eigen::Vector3d &a, const Eigen::Vector3d &b);
    /// MePoint3D 转到 Eigen::Vector3d
    static Eigen::Vector3d mePointToVector3d(MePoint3D &pt);
    /// Eigen::Vector3d 转成 MePoint3D
    static MePoint3D vector3dToMePoint(Eigen::Vector3d &vec);
    /// 将一批  Eigen::Vector3d 转成 MePoint3D
    static std::vector<MePoint3D> vector3dsToMePoints(std::vector<Eigen::Vector3d> &ptsV3);
    /// 将一批  MePoint3D 转到 Eigen::Vector3d
    static std::vector<Eigen::Vector3d> mePointsToVector3ds(std::vector<MePoint3D> &pts);
#ifndef ME_NO_CV
    static MePoint3D cvPoint3fToMePoint(cv::Point3f point);
    /// 将一批  cv::Point3d 转到 Eigen::Vector3d
    static std::vector<Eigen::Vector3d> cvPointsToVector3ds(std::vector<cv::Point3d> &pts);
    /// 将一批  cv::Point3d 转到 Eigen::Vector3d
    static void cvPointsToVector3ds(std::vector<cv::Point3d> &pts, std::vector<Eigen::Vector3d> &outPts);
    static std::vector<MePoint3D> cvPointsToMePoints(std::vector<cv::Point3d> &pts);
    /// 将一批  Eigen::Vector3d 转到 cv::Point3d
    static std::vector<cv::Point3d> vector3dsToCvPoints(std::vector<Eigen::Vector3d> &pts);
    static std::vector<cv::Point3d> mePointsToCvPoints(std::vector<MePoint3D> &pts);
    /// 将一批  cv::KeyPoint 转到 cv::Point2f
    static std::vector<cv::Point2f> cvKeyPointsToPoints(const std::vector<cv::KeyPoint> &kpts);
#endif // ME_NO_CV
    /// 给定旋转矩阵和平移t, 得到自己定义的齐次坐标
    static Matrix4d rotMatToMatrix4d(Eigen::Matrix3d &rotaionMatrix, Eigen::Vector3d &t);
    static Matrix4d rotMatToMatrix4d(Eigen::Matrix3d &rotaionMatrix);
    /// Matrix4d 获得旋转矩阵和平移矩阵
    static Eigen::Matrix3d matrix4dToRotMat(Matrix4d &m, Eigen::Vector3d *t=nullptr);
    /// Matrix4d => Eigen::Isometry3d
    static Eigen::Isometry3d matrxi4dToIsometry3d(Matrix4d &m);

    /// 给定旋转矩阵和平移t, 得到自己定义的齐次坐标
    static Eigen::Vector3d matrix4d2EigenVector(Matrix4d &m);
    /// 旋转矩阵到旋转向量转化
    static Eigen::Vector3d rotationMatrix2EigenVector(Eigen::Matrix3d &rotationMatrix);
    /// 四元数转旋转向量
    static Eigen::Vector3d quaternionToEigenVector(double w, double x, double y, double z);

    /// 旋转向量到旋转矩阵转化
    static void rotationVector2Matrix(Eigen::Vector3d &rotaionVector, Eigen::Matrix3d &rotaionMatrix);
    static Eigen::Quaterniond rotationVector2Quaternion(Eigen::Vector3d &rotationVector);

    /// 齐次矩阵和vector相乘
    static Eigen::Vector3d multiplyVector3d(Matrix4d &m, Eigen::Vector3d &vec);
    static Eigen::Vector3d multiplyVector3d(Matrix4d &m, const Eigen::Vector3d &vec);
    static void multiplyVector3d(Matrix4d &m, Eigen::Vector3d &vec, Eigen::Vector3d &outP);
    static void multiplyVector3d(Matrix4d &m, const Eigen::Vector3d &vec, Eigen::Vector3d &outP);

    /// 计算点和点之间距离
    static double distanceFrom2p(const cv::Point3f &a, const cv::Point3f &b) {
        return sqrt(pow(a.x - b.x, 2) + pow(a.y - b.y, 2) + pow(a.z - b.z, 2));
    }
    static double distanceFrom2p(const cv::Point3d &a, const cv::Point3d &b) {
        return sqrt(pow(a.x - b.x, 2) + pow(a.y - b.y, 2) + pow(a.z - b.z, 2));
    }
    static double distanceFrom2p(const cv::Point2f &a, const cv::Point2f &b) {
        return sqrt(pow(a.x - b.x, 2) + pow(a.y - b.y, 2));
    }
    static void calPointsDistance(std::vector<MePoint3D> &points, std::vector<double> &dis);
    static void calPointsDistance(std::vector<Eigen::Vector3d> &points, std::vector<double> &dis);
    static void calPointsDistance(std::vector<cv::Point3d> &points, std::vector<double> &dis);

    /// 计算在直线上的一点,
    /// offsetToP1 到p1点的距离, 方向p1->p2
    static void calPointOnLine(Eigen::Vector3d &p1, Eigen::Vector3d &p2, double offsetToP1, Eigen::Vector3d &resultP);

    /// 计算点的均值和距离rms
    static int meanStdDevPoints(const std::vector<Eigen::Vector3d>& points, Eigen::Vector3d &meanP,
                         double &mean, double& rms, double &maxV);

    /// 通过2条相交直线计算交点, l,m
    static bool intersectionLines(Eigen::Vector3d &l0, Eigen::Vector3d &l1, Eigen::Vector3d &m0, Eigen::Vector3d &m1, std::vector<Eigen::Vector3d> &outClosestPts, Eigen::Vector3d &meanClosestPt);

#ifndef ME_NO_VTK
    static void matrixToVtkMatrix(Matrix4d &m, vtkSmartPointer<vtkMatrix4x4> &vtkMatrix);
    static void matrixToVtkMatrix(Matrix4d &m, vtkMatrix4x4 &vtkMatrix);
#endif // ME_NO_VTK
};

#endif // MEEIGENMATH_H
